import ROSLIB from "roslib";

var ros = null; //Ros连接
var viewer = null; // 画布对象
var navClient = null; // 导航地图对象
var scale = 1.0; // 控制地图缩放比例：范围0.1~1
var check = false; // 首次建图调整位置的flag
// 拖拽事件监听器的句柄
var dragListener1 = null; // 事件阶段1
var dragListener2 = null; // 事件阶段2
// 导航器事件监听器的句柄
var navEventHandle = new Array();
// 导航器模式
var withOrientation = false;//true: 双击选定固定方向的模式;false: 可选方向的模式
// 网格对象
var grid = null;

/**
 * 在目标元素创建画布，元素ID为"map"，请不要修改
 * @param { Number } width 画布宽度
 * @param { Number} height 画布长度
 */
function createView(width = 500, height = 500) {
  // Create the main viewer.
  viewer = new window.ROS2D.Viewer({
    divID: "map",
    width: width,
    height: height
  });
}
/**
 * 为当前viewer绘制导航地图
 * @param { Object } link 连接对象，可传入ROSLIB.Ros对象或连接地址作为连接
 * @param { Object } self 可选，作为回调函数的参数，可传入组件环境
 * @param { Function } callback 回调函数
 */
function createNav(link, self, callback) {
  // Connect to ROS.
  if (link.roshandler != null) {
    ros = link.roshandler;
    console.log("ros in nav setted by param'roshandler'");
  } else {
    ros = new ROSLIB.Ros({
      url: link.url
    });
    console.log("ros in nav setted relying store " + link.url);
  }

  ros.on("connection", function () {
    console.log("ROS in nav: Connected to websocket server.");
  });

  ros.on("error", function (error) {
    console.log("ROS in nav: Error connecting to websocket server: ", error);
  });

  ros.on("close", function () {
    console.log("ROS in nav: Connection to websocket server closed.");
  });

  navClient = new window.ROS2D.OccupancyGridClient({
    ros: ros,
    rootObject: viewer.scene,
    continuous: true,
    topic: '/map'
  });

  navClient.on('change', function () {
    console.log("set scale: " + scale);
    viewer.scaleToDimensions(
      navClient.currentGrid.width * scale,
      navClient.currentGrid.height * scale
    );

    // console.log(navigator);
    // scale the viewer to fit the map
    // viewer.scaleToDimensions(navClient.currentGrid.width, navClient.currentGrid.height);
    // viewer.shift(navClient.currentGrid.pose.position.x, navClient.currentGrid.pose.position.y);
    if (!check) {
      viewer.scene.x -= (navClient.currentGrid.pose.position.x * viewer.scene.scaleX);
      viewer.scene.y += (navClient.currentGrid.pose.position.y * viewer.scene.scaleY);
      check = true;
    }

    callback(self);
  });

  // 添加机器人本体箭头
  buildRobotMarker();

  // 添加导航器箭头
  // buildNavigator();
}


/**
 * 用给定定位发送导航命令，并创建导航箭头
 * @param pose - the goal pose
 */
function sendGoal(pose) {
  var rootObject = viewer.scene;
  var serverName = '/move_base';
  var actionName = 'move_base_msgs/MoveBaseAction';
  var stage;
  if (rootObject instanceof window.CREATEJS.Stage) {
    stage = rootObject;
  } else {
    stage = rootObject.getStage();
  }

  // setup the actionlib client 
  var actionClient = new ROSLIB.ActionClient({
    ros: ros,
    actionName: actionName,
    serverName: serverName
  });
  // create a goal
  var goal = new ROSLIB.Goal({
    actionClient: actionClient,
    goalMessage: {
      target_pose: {
        header: {
          frame_id: '/map'
        },
        pose: pose
      }
    }
  });
  goal.send();

  // create a marker for the goal
  var goalMarker = new window.ROS2D.NavigationArrow({
    size: 15,
    strokeSize: 1,
    fillColor: window.CREATEJS.Graphics.getRGB(255, 64, 128, 0.66),
    pulse: true
  });
  goalMarker.x = pose.position.x;
  goalMarker.y = -pose.position.y;
  goalMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation);
  goalMarker.scaleX = 1.0 / stage.scaleX;
  goalMarker.scaleY = 1.0 / stage.scaleY;
  rootObject.addChild(goalMarker);

  goal.on('result', function () {
    rootObject.removeChild(goalMarker);
  });
}

/**
 * 修改地图比例
 * @param {number} newValue 不能小于零
 */
function changeScale(newValue) {
  if (newValue > 1 || newValue <= 0) {
    console.log("input illegal scale value" + newValue);
  } else {
    scale = newValue;
    viewer.scaleToDimensions(navClient.currentGrid.width * scale, navClient.currentGrid.height * scale);
  }
}
/**
 * 添加拖拽事件
 */
function addDragListener() {
  // 添加拖拽事件
  var oldX = 0;
  var oldY = 0;
  // 避免重复添加拖拽监听
  if (dragListener2 == null && dragListener1 == null) {
    dragListener1 = viewer.scene.on("mousedown", function (evt) {
      oldX = evt.stageX;
      oldY = evt.stageY;
    });

    dragListener2 = viewer.scene.on("pressmove", function (evt) {
      evt.currentTarget.x += evt.stageX - oldX;
      evt.currentTarget.y += evt.stageY - oldY;
      oldX = evt.stageX;
      oldY = evt.stageY;
    });
  }
}
/**
 * 移除拖拽事件
 */
function removeDragListener() {
  viewer.scene.off("mousedown", dragListener1);
  viewer.scene.off("pressmove", dragListener2);
  dragListener1 = null;
  dragListener2 = null;
}

/**
 * 为当前地图添加网格线，默认居中
 * @param {number} size 整个的网格的大小
 * @param {number} cellSize 网格内的单格大小
 * @param {number} lineWidth 网格线的粗细
 */
function addGird(size = 10, cellSize = 1, lineWidth = 0.005) {
  grid = new window.ROS2D.Grid({
    size: size,
    cellSize: cellSize,
    lineWidth: lineWidth
  });
  navClient.rootObject.addChild(grid);
}

/**
 * 移除当前地图的网格
 */
function removeGrid() {
  navClient.rootObject.removeChild(grid);
}

/**
 * 修改withOrientation的值
 * 修改模式:{
 *   true: 双击选定固定方向的模式,
 *   false: 可选方向的模式
 * }
 * @param {Boolean} value 布尔值。
 */
function changeOrientation(value = true) {
  withOrientation = value;
}

/**
 * 为当前地图节点创建目标箭头
 */
function buildNavigator() {
  var rootObject = viewer.scene;
  var stage;
  if (rootObject instanceof window.CREATEJS.Stage) {
    stage = rootObject;
  } else {
    stage = rootObject.getStage();
  }

  // 判断是双击选定固定方向的模式，还是可选方向的模式
  if (withOrientation == false) {
    // 双击选定固定方向的模式
    // setup a double click listener (no orientation)

    // 设置监听，并将清除监听的函数保存至navEventHandle
    navEventHandle.push(getRemoveEventListener(rootObject, 'dblclick',
      function (event) {
        // convert to ROS coordinates
        var coords = stage.globalToRos(event.stageX, event.stageY);
        var pose = new ROSLIB.Pose({
          position: new ROSLIB.Vector3(coords)
        });
        // send the goal
        sendGoal(pose);
      }));
  } else { 
    // 可选方向的模式
    // withOrientation === true
    // setup a click-and-point listener (with orientation)
    var position = null;
    var positionVec3 = null;
    var thetaRadians = 0;
    var thetaDegrees = 0;
    var orientationMarker = null;
    var mouseDown = false;
    var xDelta = 0;
    var yDelta = 0;

    // 监听鼠标事件
    var mouseEventHandler = function (event, mouseState) {

      if (mouseState === 'down') {
        // get position when mouse button is pressed down
        position = stage.globalToRos(event.stageX, event.stageY);
        positionVec3 = new ROSLIB.Vector3(position);
        mouseDown = true;
      }
      else if (mouseState === 'move') {
        // remove obsolete orientation marker
        rootObject.removeChild(orientationMarker);

        if (mouseDown === true) {
          // if mouse button is held down:
          // - get current mouse position
          // - calulate direction between stored <position> and current position
          // - place orientation marker
          var currentPos = stage.globalToRos(event.stageX, event.stageY);
          var currentPosVec3 = new ROSLIB.Vector3(currentPos);

          orientationMarker = new window.ROS2D.NavigationArrow({
            size: 25,
            strokeSize: 1,
            fillColor: window.CREATEJS.Graphics.getRGB(0, 255, 0, 0.66),
            pulse: false
          });

          xDelta = currentPosVec3.x - positionVec3.x;
          yDelta = currentPosVec3.y - positionVec3.y;

          thetaRadians = Math.atan2(xDelta, yDelta);

          thetaDegrees = thetaRadians * (180.0 / Math.PI);

          if (thetaDegrees >= 0 && thetaDegrees <= 180) {
            thetaDegrees += 270;
          } else {
            thetaDegrees -= 90;
          }

          orientationMarker.x = positionVec3.x;
          orientationMarker.y = -positionVec3.y;
          orientationMarker.rotation = thetaDegrees;
          orientationMarker.scaleX = 1.0 / stage.scaleX;
          orientationMarker.scaleY = 1.0 / stage.scaleY;

          rootObject.addChild(orientationMarker);
        }
      } else if (mouseDown) { // mouseState === 'up'
        // if mouse button is released
        // - get current mouse position (goalPos)
        // - calulate direction between stored <position> and goal position
        // - set pose with orientation
        // - send goal
        mouseDown = false;

        var goalPos = stage.globalToRos(event.stageX, event.stageY);

        var goalPosVec3 = new ROSLIB.Vector3(goalPos);

        xDelta = goalPosVec3.x - positionVec3.x;
        yDelta = goalPosVec3.y - positionVec3.y;

        thetaRadians = Math.atan2(xDelta, yDelta);

        if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
          thetaRadians += (3 * Math.PI / 2);
        } else {
          thetaRadians -= (Math.PI / 2);
        }

        var qz = Math.sin(-thetaRadians / 2.0);
        var qw = Math.cos(-thetaRadians / 2.0);

        var orientation = new ROSLIB.Quaternion({ x: 0, y: 0, z: qz, w: qw });

        var pose = new ROSLIB.Pose({
          position: positionVec3,
          orientation: orientation
        });
        // send the goal
        sendGoal(pose);
      }
    };

    // 设置监听，并将清除监听的函数保存至navEventHandle
    navEventHandle.push(getRemoveEventListener(rootObject, 'stagemousedown', function (event) {
      mouseEventHandler(event, 'down');
    }));
    navEventHandle.push(getRemoveEventListener(rootObject, 'stagemousemove', function (event) {
      mouseEventHandler(event, 'move');
    }));
    navEventHandle.push(getRemoveEventListener(rootObject, 'stagemouseup', function (event) {
      mouseEventHandler(event, 'up');
    }));
  }
}

/**
 * 清除所有监听事件。这些监听事件在被添加时由navEventHandle捕获
 */
function removeNavigator() {
  navEventHandle.forEach(function (currentValue) {
    currentValue.destroy();
  });
  navEventHandle = [];
}

/**
 * 仅内部使用，返回匿名监听函数的handle，用于取消监听
 * @param {Object} node 添加监听事件的目标
 * @param {String} type 监听事件的类型
 * @param {Function} callback 监听事件的回调函数
 */
function getRemoveEventListener(node, type, callback) {
  node.addEventListener(type, callback);
  return {
    destroy() {
      node.removeEventListener(type, callback);
    },
  };
}



/**
 * 创建机器人本体的箭头
 */
function buildRobotMarker() {
  var rootObject = viewer.scene;
  // get a handle to the stage
  var stage;
  if (rootObject instanceof window.CREATEJS.Stage) {
    stage = rootObject;
  } else {
    stage = rootObject.getStage();
  }

  // marker for the robot
  var robotMarker = new window.ROS2D.NavigationArrow({
    size: 25,
    strokeSize: 1,
    fillColor: window.CREATEJS.Graphics.getRGB(255, 128, 0, 0.66),
    pulse: false
  });

  // wait for a pose to come in first
  robotMarker.visible = false;
  rootObject.addChild(robotMarker);
  // var initScaleSet = false;

  // setup a listener for the robot pose
  var poseListener = new ROSLIB.Topic({
    ros: ros,
    name: '/robot_pose',
    messageType: 'geometry_msgs/Pose',
    throttle_rate: 100
  });
  poseListener.subscribe(function (pose) {
    // update the robots position on the map
    robotMarker.x = pose.position.x;
    robotMarker.y = -pose.position.y;

    console.log(stage.scaleX);
    console.log(stage.scaleY);

    robotMarker.scaleX = 1.0 / stage.scaleX;
    robotMarker.scaleY = 1.0 / stage.scaleY;

    // if (!initScaleSet) {
    //   robotMarker.scaleX = 1.0 / stage.scaleX;
    //   robotMarker.scaleY = 1.0 / stage.scaleY;
    //   initScaleSet = true;
    // }

    // change the angle
    robotMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation);

    robotMarker.visible = true;
  });
}


/**
 * 完全置空所有连接、主题和地图
 */
function destroy() {
  ros.close();
  ros = null;
  viewer = null;
  // nav = null;
  check = false;
  scale = 1.0;
  withOrientation = false;
  console.log("viewer has been closed");
  removeDragListener();
  removeNavigator();
}

export { createView, createNav, destroy, buildNavigator, buildRobotMarker, removeNavigator, changeScale, addDragListener, removeDragListener, changeOrientation, addGird, removeGrid };
